#ifndef _GNU_SOURCE
#define _GNU_SOURCE
#endif
#include "motionapi.h"
#include "motion.h"
#include "mot_priv.h"

#include <stdarg.h>
#include <pthread.h>
#include <sys/time.h>
#include <unistd.h>
#include <stdint.h>
#include <string.h>
#include <sched.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <arpa/inet.h>
#include <netinet/tcp.h>
#include <semaphore.h>

#include "rtapi/rtapi.h"        /* RTAPI realtime OS API */
#include "rtapi/rtapi_app.h"    /* RTAPI realtime module decls */
#include "rtapi/rtapi_string.h" /* memset */
#include "hal/hal.h"            /* decls for HAL implementation */
#include "motion_debug.h"
#include "motion_struct.h"
#include "rtapi/rtapi_math.h"
#include "errcode.h"
#include "motion/semintf.h"
#include "dyna/jaka_dyn_intf.h"
//#include "tp/tpmanager.hh"
#include "motionItf.h"
#include "remoteApi/usrRemote.h"
#include "jkutil/timespec.h"
#include "command.h"
#include "rtdev/rtdev.h"
#include "rtdev/safetyboard/scbitf.h"
#include "rtdev/bp/bpcontrolhandle.h"
#include "rtbus/rtbus.h"
#include "axis/axis.h"
#include "axisgroup/axisgroup.h"
#include "log/zlog.hh"
#include "rtdev/rtrobot.h"
#include "axisgroup/axisgroup_robot.h"
#include "axisgroup/robot_mgr.h"
#include "safety/io/safeio.h"
#include "axisgroup/safety_event.h"

// Mark strings for translation, but defer translation to userspace
#define _(s) (s)
#define G 9.8
#define CAB3_0 4
#define CAB2_1 2

#ifdef BUFFERLEN
#undef BUFFERLEN
#endif
#ifndef BUFFERLEN
#define BUFFERLEN 1024
#endif

/***********************************************************************
*                    KERNEL MODULE PARAMETERS                          *
************************************************************************/
static zucmot_hal_data_t hal_data_memory_;

/* RTAPI shmem key - for comms with higher level user space stuff */
static int key = DEFAULT_SHMEM_KEY; /* the shared memory key, default value */
// RTAPI_MP_INT(key, "shared memory key");
static int extioKey = DEFAULT_SHMEM_KEY + 100;
// RTAPI_MP_INT(extioKey, "shared memory key for extio");
static int sensorKey = DEFAULT_SHMEM_KEY + 101;
// RTAPI_MP_INT(sensorKey, "shared memory key for sensor");
static int servoJKey = DEFAULT_SHMEM_KEY + 102;
// RTAPI_MP_INT(servoJKey, "shared memory key for servoj");
static int physicIOKey = DEFAULT_SHMEM_KEY + 103;
// RTAPI_MP_INT(physicIOKey, "shared memory key for physic io");
static int pndevKey = DEFAULT_SHMEM_KEY + 104;
// RTAPI_MP_INT(pndevKey, "shared memory key for PNDev");
static int eipKey = DEFAULT_SHMEM_KEY + 105;
// RTAPI_MP_INT(eipKey, "shared memory key for ethernet/IP adapter");
static int superKey = DEFAULT_SHMEM_KEY + 106;

// RTAPI_MP_LONG(base_period_nsec, "fastest thread period (nsecs)");
int base_thread_fp = 0; /* default is no floating point in base thread */
// RTAPI_MP_INT(base_thread_fp, "floating point in base thread?");

static long servo_period_nsec = 8000000; /* servo thread period */
// RTAPI_MP_LONG(servo_period_nsec, "servo thread period (nsecs)");
static long traj_period_ratio = 1;      // 规划器周期相对与rtbus周期的倍数
static long traj_period_nsec = 8000000; /* trajectory planner period */

// RTAPI_MP_LONG(traj_period_nsec, "trajectory planner period (nsecs)");
static int num_joints = ZUCMOT_MAX_JOINTS; /* default number of joints present */
// RTAPI_MP_INT(num_joints, "number of joints");
static int cab_type = 2;
// RTAPI_MP_INT(cab_type, "type of cabinet: 1 for cab v1.0, 2 for cab v2.1")
static int unlock_joints_mask = 0; /* mask to select joints for unlock pins */
// RTAPI_MP_INT(unlock_joints_mask, "mask to select joints for unlock pins");

static int num_dio = DEFAULT_DIO_CAB_V21; /* default number of cab DIO */
static int num_ai = DEFAULT_AIO;          /* default number of cab AI */
static int num_ao = DEFAULT_AIO;          /* default number of cab AO */

/***********************************************************************
*                  GLOBAL VARIABLE DEFINITIONS                         *
************************************************************************/

/* pointer to zucmot_hal_data_t struct in HAL shmem, with all HAL data */
zucmot_hal_data_t* zucmot_hal_data = 0;

/* pointer to motor data*/
zucmot_motor_t* motors = 0;

/* pointer to joint data */
zucmot_joint_t* joints = 0;

/* pointer to axis data */
zucmot_axis_t* axes = 0;

#ifndef STRUCTS_IN_SHMEM
zucmot_joint_t motor_array[ZUCMOT_MAX_JOINTS];
/* allocate array for joint data */
zucmot_joint_t joint_array[ZUCMOT_MAX_JOINTS];
/* allocate array for axis data */
zucmot_axis_t axis_array[ZUCMOT_MAX_AXIS];
#endif

/*
  Principles of communication:

  Data is copied in or out from the various types of comm mechanisms:
  mbuff mapped memory for Linux/RT-Linux, or OS shared memory for Unixes.

  zucmotStruct is ptr to this memory.

  zucmotCommand points to zucmotStruct->command,
  zucmotStatus points to zucmotStruct->status,
  zucmotError points to zucmotStruct->error, and
 */
static zucmot_struct_t zucmot_struct_data_;
static PhysicIOShm_t zucmot_PhysicIO_data_;
ModbusIOShm_t zucmot_modbusio_data_;
struct SensorShm zucmot_sensor_data_;
struct PNDevShm zucmot_pndev_data_;
struct EIPShm zucmot_eipdev_data_;
struct TorqueControlShm zucmot_torque_control_data_;

zucmot_struct_t* zucmotStruct = 0;
/* ptrs to either buffered copies or direct memory for command and status */
struct zucmot_command_t* zucmotCommand = 0;
struct zucmot_status_t* zucmotStatus = 0;
struct zucmot_config_t* zucmotConfig = 0;
struct zucmot_debug_t* zucmotDebug = 0;
struct zucmot_error_t* zucmotError = 0; /* unused for RT_FIFO */
//
ModbusIOShm_t* zucmotExtIOStruct = NULL;
PhysicIOShm_t* zucmotPhysicIOStruct = NULL;

struct SensorShm* zucmotSensorStruct = 0;
struct PNDevShm* zucmotPNDevStruct = NULL;
struct EIPShm* zucmotEIPStruct = NULL;
struct TorqueControlShm* zucmotTorqueControlStruct = NULL;

/***********************************************************************
*                  LOCAL VARIABLE DECLARATIONS                         *
************************************************************************/

/* RTAPI shmem ID - for comms with higher level user space stuff */
static int zuc_shmem_id;                /* the shared memory ID */
static int zuc_extio_shmem_id;          /* the shared memory ID for extio*/
static int zuc_sensor_shmem_id;         /* the shared memory ID for sensor*/
static int zuc_servoj_shmem_id;         /* the shared memory ID for servoj*/
static int zuc_pndev_shmem_id;          /* the shared memory ID for profinet*/
static int zuc_eip_shmem_id;            /* the shared memory ID for ethernet/ip*/
static int zuc_physic_io_shmem_id;      /* the shared memory ID for physic io*/
static int zuc_torque_control_shmem_id; /* the shared memory ID for super*/
/***********************************************************************
*                   LOCAL FUNCTION PROTOTYPES                          *
************************************************************************/

/* init_hal_io() exports HAL pins and parameters making data from
   the realtime control module visible and usable by the world
*/
static int init_hal_io(void);

/* init_comm_buffers() allocates and initializes the command,
   status, and error buffers used to communicate witht the user
   space parts of zuc.
*/
static int init_comm_buffers(void);

/* functions called by init_comm_buffers() */

/* init_threads() creates realtime threads, exports functions to
   do the realtime control, and adds the functions to the threads.
*/
static int init_threads(void);

/* functions called by init_threads() */
static int setTrajCycleTime(double secs);
static int setServoCycleTime(double secs);
static int init_rtbus_thread();
/***********************************************************************
*                     PUBLIC FUNCTION CODE                             *
************************************************************************/

void switch_to_teleop_mode(void)
{
    // int joint_num;
    // zucmot_joint_t* joint;

    // if (zucmotConfig->kinType != KINEMATICS_IDENTITY)
    // {
    //     if (!checkAllHomed())
    //     {
    //         return;
    //     }
    // }

    // for (joint_num = 0; joint_num < zucmotConfig->numJoints; joint_num++)
    // {
    //     joint = &joints[joint_num];
    //     if (joint != 0)
    //     {
    //         joint->free_tp.enable = 0;
    //     }
    // }

    // zucmotDebug->teleoperating = 1;
    // zucmotDebug->coordinating = 0;
    // zucmotDebug->teleoperating_tool = 0;
    // zucmotDebug->dragging = 0;
    // // zucmotDebug->servomoveoperating = 0;
    // zucmotDebug->admitting = 0;
}

void switch_to_teleop_tool_mode(void)
{
    // int joint_num;
    // zucmot_joint_t* joint;

    // if (zucmotConfig->kinType != KINEMATICS_IDENTITY)
    // {
    //     if (!checkAllHomed())
    //     {
    //         return;
    //     }
    // }

    // for (joint_num = 0; joint_num < zucmotConfig->numJoints; joint_num++)
    // {
    //     joint = &joints[joint_num];
    //     if (joint != 0)
    //     {
    //         joint->free_tp.enable = 0;
    //     }
    // }

    // zucmotDebug->teleoperating = 0;
    // zucmotDebug->coordinating = 0;
    // zucmotDebug->teleoperating_tool = 1;
    // zucmotDebug->dragging = 0;
    // // zucmotDebug->servomoveoperating = 0;
    // zucmotDebug->admitting = 0;
}

void switch_to_servojop_mode()
{
    // int axis_num;
    // zucmot_axis_t* axis;

    // for (axis_num = 0; axis_num < ZUCMOT_MAX_AXIS; axis_num++)
    // {
    //     axis = &axes[axis_num];
    //     if (axis != 0)
    //     {
    //         axis->teleop_tp.enable = 0;
    //     }
    // }

    // zucmotDebug->teleoperating = 0;
    // zucmotDebug->coordinating = 0;
    // zucmotDebug->teleoperating_tool = 0;
    // zucmotDebug->dragging = 0;
    // // zucmotDebug->servomoveoperating = 1;
    // zucmotDebug->admitting = 0;
    // rtapi_print("switch_to_servojop_mode called!\n");
}

void switch_to_admittance_mode()
{
    // int axis_num;
    // zucmot_axis_t* axis;

    // for (axis_num = 0; axis_num < ZUCMOT_MAX_AXIS; axis_num++)
    // {
    //     axis = &axes[axis_num];
    //     if (axis != 0)
    //     {
    //         axis->teleop_tp.enable = 0;
    //     }
    // }

    // zucmotDebug->teleoperating = 0;
    // zucmotDebug->coordinating = 0;
    // zucmotDebug->teleoperating_tool = 0;
    // zucmotDebug->dragging = 0;
    // // zucmotDebug->servomoveoperating = 0;
    // zucmotDebug->admitting = 1;
    // //rtapi_print("switch_to_admittance_mode called!\n");
}

void switch_to_drag_mode(void)
{
    // int joint_num;
    // zucmot_joint_t* joint;

    // for (joint_num = 0; joint_num < zucmotConfig->numJoints; joint_num++)
    // {
    //     joint = &joints[joint_num];
    //     if (joint != 0)
    //     {
    //         joint->free_tp.enable = 0;
    //     }
    // }

    // zucmotDebug->teleoperating = 0;
    // zucmotDebug->coordinating = 0;
    // zucmotDebug->teleoperating_tool = 0;
    // zucmotDebug->dragging = 1;
    // // zucmotDebug->servomoveoperating = 0;
    // zucmotDebug->admitting = 0;
}

void zucmot_config_change(void)
{
    if (zucmotConfig->head == zucmotConfig->tail)
    {
        zucmotConfig->config_num++;
        zucmotStatus->config_num = zucmotConfig->config_num;
        zucmotConfig->head++;
    }
}

void reportInfo(int infocode, const char* fmt, ...)
{
    char buf[BUFFERLEN + 1];
    // int len;
    va_list args;

    va_start(args, fmt);
    zucmotErrorPutfv(zucmotError, 1, infocode, fmt, args);
    vsnprintf(buf, BUFFERLEN, fmt, args);
    va_end(args);
}

void reportWarning(int warncode, const char* fmt, ...)
{
    char buf[BUFFERLEN + 1];
    // int len;
    va_list args;

    va_start(args, fmt);
    zucmotErrorPutfv(zucmotError, 2, warncode, fmt, args);
    vsnprintf(buf, BUFFERLEN, fmt, args);
    va_end(args);
}

void reportError(int errcode, const char* fmt, ...)
{
    char buf[BUFFERLEN + 1];
    // int len;
    va_list args;

    va_start(args, fmt);
    zucmotErrorPutfv(zucmotError, 3, errcode, fmt, args);
    vsnprintf(buf, BUFFERLEN, fmt, args);
    va_end(args);

    zucmot_hal_data->error_triggered = 1;
    zucmot_hal_data->motion_error_code = errcode;
}

#ifndef va_copy
#define va_copy(dest, src) ((dest) = (src))
#endif

// static rtapi_msg_handler_t old_handler = NULL;
// static void zuc_message_handler(msg_level_t level, const char* fmt, va_list ap)
// {
//     va_list apc;
//     va_copy(apc, ap);
//     if (level == RTAPI_MSG_ERR)
//         zucmotErrorPutfv(zucmotError, 0, 0, fmt, apc);
//     if (old_handler)
//         old_handler(level, fmt, ap);
//     va_end(apc);
// }
int motion::start_motion()
{
    int retval = init_threads();
    if (retval != 0)
    {
        rtapi_print_msg(RTAPI_MSG_ERR, _("MOTION: init_threads() failed\n"));
        return -1;
    }
    rtbus::BusMgr::get()->enable_device(true);
    return retval;
}
int motion::init_motion(int cabType_)
{
    cab_type = cabType_;

    int retval;
    rtapi_print_msg(RTAPI_MSG_INFO, "MOTION: init_module() starting...\n");
    if ((num_joints < 1) || (num_joints > ZUCMOT_MAX_JOINTS))
    {
        rtapi_print_msg(RTAPI_MSG_ERR, _("MOTION: num_joints is %d, must be between 1 and %d\n"), num_joints, ZUCMOT_MAX_JOINTS);
        return -1;
    }

    /* initialize/export HAL pins and parameters */
    retval = init_hal_io();
    if (retval != 0)
    {
        rtapi_print_msg(RTAPI_MSG_ERR, _("MOTION: init_hal_io() failed\n"));
        return -1;
    }

    /* allocate/initialize user space comm buffers (cmd/status/err) */
    retval = init_comm_buffers();
    if (retval != 0)
    {
        rtapi_print_msg(RTAPI_MSG_ERR, _("MOTION: init_comm_buffers() failed\n"));
        return -1;
    }

    // rtbus::BusMgr::get()->init_all_bus();
    init_rtbus_thread();  //FIXME:放到rtbus中去

    //单轴初始化
    for (size_t i = 0; i < axis::AxisMgr::get().count(); i++)
    {
        auto a = axis::AxisMgr::get().axis(i);
        if (!a)
        {
            break;
        }
        a->init();  // 此处初始化jkservo
    }

    //轴组初始化
    axisgroup::AxisGroupMgr::instance().reset();

    /* set up for realtime execution of code */

    rtdev::get_bp_handle()->boot_init();

    // 启动后，需要查询SCB软硬件版本，确认是否与启动时识别的一致
    // query_scb_hw_version();
    // query_scb_sw_release_verion();

    return 0;
}

/***********************************************************************
*                         LOCAL FUNCTION CODE                          *
************************************************************************/

/* init_hal_io() exports HAL pins and parameters making data from
   the realtime control module visible and usable by the world
*/
static int init_hal_io(void)
{
    int n, retval;
    joint_hal_t* joint_data;
    axis_hal_t* axis_data;
    int user_dio_num = ZUCMOT_ZLAN_DIO_NUM;
    int user_ai_num = ZUCMOT_ZLAN_AI_NUM;
    int user_ao_num = ZUCMOT_ZLAN_AO_NUM;

    num_dio = ZUCMOT_ZLAN_DIO_NUM + ZUCMOT_MAX_MODBUS_DIO + ZUCMOT_MAX_PNDev_DIO;
    if (cab_type == 2)
    {
        num_dio = ZUCMOT_SCB_USER_DIO_NUM + ZUCMOT_MAX_MODBUS_DIO + ZUCMOT_MAX_PNDev_DIO;
    }
    else if (cab_type == 3)
    {
        num_dio = ZUCMOT_MINI_USER_DIO_NUM + ZUCMOT_MAX_MODBUS_DIO + ZUCMOT_MAX_PNDev_DIO;
    }

    if ((num_dio < 1) || (num_dio > ZUCMOT_MAX_DIO))
    {
        rtapi_print_msg(RTAPI_MSG_ERR, _("MOTION: num_dio is %d, must be between 1 and %d\n"), num_dio, ZUCMOT_MAX_DIO);
        return -1;
    }

    num_ai = ZUCMOT_ZLAN_AI_NUM + ZUCMOT_MAX_MODBUS_AIO_NUM + ZUCMOT_PNDev_INT_AIO_NUM + ZUCMOT_PNDev_FLOAT_AIO_NUM;
    num_ao = ZUCMOT_ZLAN_AO_NUM + ZUCMOT_MAX_MODBUS_AIO_NUM + ZUCMOT_PNDev_INT_AIO_NUM + ZUCMOT_PNDev_FLOAT_AIO_NUM;
    if (cab_type == 2)
    {
        num_ai = num_ao = ZUCMOT_SCB_USER_AIO_NUM + ZUCMOT_MAX_MODBUS_AIO_NUM + ZUCMOT_PNDev_INT_AIO_NUM + ZUCMOT_PNDev_FLOAT_AIO_NUM;
    }
    else if (cab_type == 3)
    {
        num_ai = num_ao = ZUCMOT_MAX_MODBUS_AIO_NUM + ZUCMOT_PNDev_INT_AIO_NUM + ZUCMOT_PNDev_FLOAT_AIO_NUM;
    }
    if ((num_ai < 1) || (num_ao < 1) || (num_ao > ZUCMOT_MAX_AIO) || (num_ai > ZUCMOT_MAX_AIO))
    {
        rtapi_print_msg(RTAPI_MSG_ERR, _("MOTION: num_ai or num_ao must be between 1 and %d\n"), ZUCMOT_MAX_AIO);
        return -1;
    }
    rtapi_print_msg(RTAPI_MSG_INFO, "MOTION: init_hal_io() starting...\n");

    /* allocate shared memory for machine data */
    zucmot_hal_data = &hal_data_memory_;

    if (cab_type == 2)
    {
        user_dio_num = ZUCMOT_SCB_USER_DIO_NUM;
        user_ai_num = user_ao_num = ZUCMOT_SCB_USER_AIO_NUM;
    }
    else if (cab_type == 3)
    {
        user_dio_num = ZUCMOT_MINI_USER_DIO_NUM;
        user_ai_num = user_ao_num = ZUCMOT_SCB_USER_AIO_NUM;
    }

    /* default value of enable is TRUE, so simple machines can leave it disconnected */
    zucmot_hal_data->zuc_enable_cmd = 0;
    zucmot_hal_data->zuc_enable_in = 0;
    // *(zucmot_hal_data->ioaio_conn) = 0;

    for (n = 0; n < user_dio_num; n++)
    {
        zucmot_hal_data->physical_di[n] = 0;
        zucmot_hal_data->physical_do[n] = 0;
    }
    for (n = 0; n < ZUCMOT_MAX_MODBUS_DIO; n++)
    {
        zucmot_hal_data->mdbus_di[n] = 0;
        zucmot_hal_data->mdbus_do[n] = 0;
    }
    // if(cab_type>1){
    //     for (n = 0; n < (ZUCMOT_TOTAL_SCB_DIO_NUM-ZUCMOT_SCB_USER_DIO_NUM); n++) {
    //         *(zucmot_hal_data->scb_inner_di[n]) = 0;
    //         *(zucmot_hal_data->scb_inner_do[n]) = 0;
    //     }
    // }

    for (n = 0; n < user_ai_num; n++) { zucmot_hal_data->physical_ai[n] = 0; }
    for (n = 0; n < user_ao_num; n++) { zucmot_hal_data->physical_ao[n] = 0; }

    zucmot_hal_data->program_run_req = 0;
    zucmot_hal_data->program_pause_req = 0;
    zucmot_hal_data->collision_program_pause_req = 0;
    zucmot_hal_data->program_resume_req = 0;
    zucmot_hal_data->program_stop_req = 0;
    zucmot_hal_data->power_on_req = 0;
    zucmot_hal_data->power_off_req = 0;
    zucmot_hal_data->servo_enable_req = 0;
    zucmot_hal_data->bp_servo_enable_req = 0;
    zucmot_hal_data->delay_enable_req = 0;
    zucmot_hal_data->pro_autorun_req = 0;

    zucmot_hal_data->servo_disable_req = 0;
    zucmot_hal_data->percentage_mode_level1_req = 0;
    zucmot_hal_data->stop_mode_req = 0;
    zucmot_hal_data->safetyguard_req = 0;
    zucmot_hal_data->percentage_mode_level2_req = 0;
    zucmot_hal_data->reduce_mode_req = 0;
    zucmot_hal_data->collision_lv0_req = 0;
    zucmot_hal_data->collision_lv1_req = 0;
    zucmot_hal_data->collision_lv2_req = 0;
    zucmot_hal_data->collision_lv3_req = 0;
    zucmot_hal_data->collision_lv4_req = 0;
    zucmot_hal_data->collision_lv5_req = 0;
    zucmot_hal_data->collision_lv7_req = 0;
    zucmot_hal_data->return_to_initial_position = 0;  //
    zucmot_hal_data->sys_shutdown = 0;
    zucmot_hal_data->clear_error_req = 0;  //
    zucmot_hal_data->drag_mode_req = 0;
    zucmot_hal_data->drag_mode_exit_req = 0;
    zucmot_hal_data->drag_mode_entr_req = 0;

    zucmot_hal_data->motors_servo_on = 0;
    zucmot_hal_data->motion_enabled = 0;
    zucmot_hal_data->in_position = 0;
    zucmot_hal_data->motion_type = 0;
    zucmot_hal_data->coord_mode = 0;
    zucmot_hal_data->teleop_mode = 0;
    zucmot_hal_data->teleop_tool_mode = 0;
    zucmot_hal_data->drag_mode = 0;
    zucmot_hal_data->coord_error = 0;
    zucmot_hal_data->error_triggered = 0;
    zucmot_hal_data->on_soft_limit = 0;
    zucmot_hal_data->next_to_limit = 0;
    /* servo status initialization*/
    for (n = 0; n < 6; n++)
    {
        zucmot_hal_data->servoVersionH[n] = 0;
        zucmot_hal_data->servoVersionL[n] = 0;

        zucmot_hal_data->robotSerialNumH[n] = 0;
        zucmot_hal_data->instVoltageH[n] = 0;
        zucmot_hal_data->instTemperatureH[n] = 0;
        zucmot_hal_data->actualPositionH[n] = 0;
        zucmot_hal_data->actualTorqueH[n] = 0;
        zucmot_hal_data->errorCodeH[n] = 0;
        zucmot_hal_data->errorStateH[n] = 0;
        zucmot_hal_data->enableStateH[n] = 0;
        zucmot_hal_data->collisionStateH[n] = 0;

        zucmot_hal_data->robotSerialNumL[n] = 0;
        zucmot_hal_data->instVoltageL[n] = 0;
        zucmot_hal_data->instTemperatureL[n] = 0;
        zucmot_hal_data->actualPositionL[n] = 0;
        zucmot_hal_data->actualTorqueL[n] = 0;
        zucmot_hal_data->errorCodeL[n] = 0;
        zucmot_hal_data->errorStateL[n] = 0;
        zucmot_hal_data->enableStateL[n] = 0;
        zucmot_hal_data->collisionStateL[n] = 0;
    }

    /* robot status*/
    for (n = 0; n < 6; n++)
    {
        zucmot_hal_data->tool_offsetH[n] = 0;
        zucmot_hal_data->tool_offsetL[n] = 0;
        zucmot_hal_data->tcp_velocityH[n] = 0;
        zucmot_hal_data->tcp_velocityL[n] = 0;
    }
    zucmot_hal_data->tcp_compound_velH = 0;
    zucmot_hal_data->tcp_compound_velL = 0;
    zucmot_hal_data->motion_error_code = 0;

    /* export joint pins and parameters */
    for (n = 0; n < num_joints; n++)
    {
        zucmot_hal_data->joint[n].amp_enable = 0;
        zucmot_hal_data->joint[n].home_state = 0;
    }

    rtapi_print_msg(RTAPI_MSG_INFO, "MOTION: init_hal_io() complete, %d axes.\n", n);
    return 0;
}

/* init_comm_buffers() allocates and initializes the command,
   status, and error buffers used to communicate with the user
   space parts of zuc.
*/
static int init_comm_buffers(void)
{
    int sem_id;
    int joint_num, axis_num, n;
    zucmot_joint_t* joint;
    zucmot_motor_t* motor;
    int retval;
    DHParam iniDHParam = {0};

    rtapi_print_msg(RTAPI_MSG_INFO, "MOTION: init_comm_buffers() starting...\n");

    zucmotStruct = &zucmot_struct_data_;
    zucmotDebug = 0;
    zucmotStatus = 0;
    zucmotCommand = 0;
    zucmotConfig = 0;
    zucmotExtIOStruct = &zucmot_modbusio_data_;
    zucmotPhysicIOStruct = &zucmot_PhysicIO_data_;

    /* allocate and initialize the shared memory for sensor*/
    zucmotSensorStruct = &zucmot_sensor_data_;
    sem_id = sem_get(SENSOR_SEM_KEY);
    if (sem_p(sem_id) == 0)
    {
        /* reset the memory in initialization*/
        memset(zucmotSensorStruct, 0, sizeof(SensorShm));
        zucmotSensorStruct->sensorConfig.mode = 0;
        zucmotSensorStruct->sensorStatus.status = 0;
        zucmotSensorStruct->sensorStatus.errCode = 0;
        sem_v(sem_id);
    }

    // 新增叠加接口
    zucmotTorqueControlStruct = &zucmot_torque_control_data_;
    sem_id = sem_get(TORQUE_CONTROL_SEM_KEY);
    if (sem_p(sem_id) == 0)
    {
        /* reset the memory in initialization*/
        memset(zucmotTorqueControlStruct, 0, sizeof(TorqueControlShm));
        zucmotTorqueControlStruct->torque_que.front = -1;
        zucmotTorqueControlStruct->torque_que.rear = -1;
        rtapi_print("TorqueControlShm ptr = %p\n", zucmotTorqueControlStruct);
        sem_v(sem_id);
    }

    /* allocate and initialize the shared memory for PN Device*/
    zucmotPNDevStruct = &zucmot_pndev_data_;
    sem_id = sem_get(PNDEV_SEM_KEY);
    if (sem_p(sem_id) == 0)
    {
        /* reset the memory in initialization*/
        memset(zucmotPNDevStruct, 0, sizeof(PNDevShm));

        sem_v(sem_id);
    }

    /* allocate and initialize the shared memory for ethernet/ip */
    zucmotEIPStruct = &zucmot_eipdev_data_;
    sem_id = sem_get(EIP_SEM_KEY);
    if (sem_p(sem_id) == 0)
    {
        /* reset the memory in initialization*/
        memset(zucmotEIPStruct, 0, sizeof(EIPShm));
        sem_v(sem_id);
    }

    // zucmot_IODevInstance(IODevTYPE_PHYSIC, zuc_physic_io_shmem_id);
    // zucmot_IODevInstance(IODevTYPE_SYNCH_MB_SLAVE, zuc_extio_shmem_id);
    // zucmot_IODevInstance(IODevTYPE_EXTIO, zuc_extio_shmem_id);
    // zucmot_IODevInstance(IODevTYPE_SYNCH_PN_DEV, zuc_pndev_shmem_id);
    // zucmot_IODevInstance(IODevTYPE_SYNCH_EIP_ADPT, zuc_eip_shmem_id);

    /* zero shared memory before doing anything else. */
    memset(zucmotStruct, 0, sizeof(zucmot_struct_t));

    /* we'll reference zucmotStruct directly */
    zucmotCommand = &zucmotStruct->command;
    zucmotStatus = &zucmotStruct->status;
    zucmotConfig = &zucmotStruct->config;
    zucmotDebug = &zucmotStruct->debug;
    zucmotError = &zucmotStruct->error;

    /* init error struct */
    zucmotErrorInit(zucmotError);

    /* init command struct */
    zucmotCommand->head = 0;
    zucmotCommand->command = cmd_code_t::ZUCMOT_NONE;
    zucmotCommand->commandNum = 0;
    zucmotCommand->tail = 0;

    /* init status struct */
    zucmotStatus->head = 0;
    zucmotStatus->commandEcho = cmd_code_t::ZUCMOT_NONE;
    zucmotStatus->commandNumEcho = 0;
    zucmotStatus->commandStatus = cmd_status_t::ZUCMOT_COMMAND_OK;

    /* init more stuff */
    zucmotDebug->head = 0;
    zucmotConfig->head = 0;

    zucmotStatus->motionFlag = 0;
    SET_MOTION_ERROR_FLAG(0);
    SET_MOTION_COORD_FLAG(0);
    SET_MOTION_TELEOP_FLAG(0);
    SET_MOTION_TELEOP_TOOL_FLAG(0);
    SET_MOTION_DRAG_FLAG(0)
    SET_MOTION_SERVOMOVEOP_FLAG(0);
    SET_MOTION_ADMITTANCE_FLAG(0);
    zucmotDebug->split = 0;
    zucmotDebug->re_enable_delay = 0;

    zucmotStatus->heartbeat = 0;
    // 初始默认时最大轴数
    zucmotConfig->numJoints = num_joints;
    zucmotConfig->cabTotalNumDIO = num_dio;
    zucmotConfig->cabTotalNumAI = num_ai;
    zucmotConfig->cabTotalNumAO = num_ao;
    zucmotConfig->cabUsrPhysNumDIO = num_dio - ZUCMOT_MAX_MODBUS_DIO - ZUCMOT_MAX_PNDev_DIO;
    zucmotConfig->cabUsrPhysNumAI = num_ai - ZUCMOT_MAX_MODBUS_AIO_NUM - ZUCMOT_PNDev_INT_AIO_NUM - ZUCMOT_PNDev_FLOAT_AIO_NUM;  //
    zucmotConfig->cabUsrPhysNumAO = num_ao - ZUCMOT_MAX_MODBUS_AIO_NUM - ZUCMOT_PNDev_INT_AIO_NUM - ZUCMOT_PNDev_FLOAT_AIO_NUM;

    // zucmot_SetCabTotalNumAO(zucmotConfig->cabTotalNumAO);
    // zucmot_SetCabTotalNumDIO(zucmotConfig->cabTotalNumDIO);
    // zucmot_SetCabTotalNumAI(zucmotConfig->cabTotalNumAI);

    // zucmot_SetCabUsrPhysNumAI(zucmotConfig->cabUsrPhysNumAI);
    // zucmot_SetCabUsrPhysNumAO(zucmotConfig->cabUsrPhysNumAO);
    // zucmot_SetCabUsrPhysNumDIO(zucmotConfig->cabUsrPhysNumDIO);

    zucmotConfig->TIO_TotalNumDIO = ZUCMOT_MAX_TIO_DIO;
    zucmotConfig->MB_Slave_TotalNumDIO = ZUCMOT_MAX_MODBUS_DIO;
    zucmotConfig->MB_Slave_TotalNumAIO = ZUCMOT_MAX_MODBUS_AIO_NUM;
    zucmotConfig->PN_Dev_TotalNumDIO = ZUCMOT_MAX_PNDev_DIO;
    zucmotConfig->PN_Dev_TotalNumAIO = ZUCMOT_MAX_PNDev_AIO_NUM;
    zucmotConfig->EIP_Adpt_TotalNumDIO = ZUCMOT_MAX_EIP_DIO;
    zucmotConfig->EIP_Adpt_TotalNumAIO = ZUCMOT_MAX_EIP_AIO_NUM;

    // zucmot_SetMB_Slave_TotalNumDIO(zucmotConfig->MB_Slave_TotalNumDIO);
    // zucmot_SetMB_Slave_TotalNumAIO(zucmotConfig->MB_Slave_TotalNumAIO);

    // zucmot_SetPN_Dev_TotalNumDIO(zucmotConfig->PN_Dev_TotalNumDIO);
    // zucmot_SetPN_Dev_TotalNumAIO(zucmotConfig->PN_Dev_TotalNumAIO);

    // zucmot_SetEIP_Adpt_TotalNumDIO(zucmotConfig->EIP_Adpt_TotalNumDIO);
    // zucmot_SetEIP_Adpt_TotalNumAIO(zucmotConfig->EIP_Adpt_TotalNumAIO);

    zucmotConfig->cab_type = cab_type;
    // zucmot_SetCabType(cab_type);
    zucmotConfig->safetyCfg.clsnOpt.reboundLength = 3.0;
    zucmotConfig->safetyCfg.clsnOpt.rebound_j_acc = 720.0;
    zucmotConfig->safetyCfg.clsnOpt.rebound_j_vel = 180.0;
    zucmotConfig->safetyCfg.clsnOpt.rebound_l_acc = 2000.0;
    zucmotConfig->safetyCfg.clsnOpt.rebound_l_vel = 1000.0;
    zucmotConfig->safetyCfg.clsnOpt.collisionMethod = 0;
    zucmotConfig->safetyCfg.clsnOpt.reboundAngle = 3.0;
    zucmotConfig->safetyCfg.clsnOpt.reactionType = 0;
    zucmotConfig->safetyCfg.init_jpos_tol = 0.1;
    zucmotStatus->servoDHState = 0;
    zucmotStatus->servoDynState = 0;
    zucmotStatus->protective_stop = 0;
    zucmotStatus->last_convayorCnt = 0;
    zucmotStatus->turns_count = 0;
    zucmotStatus->safezone_confirm = 0;
    zucmotStatus->attitude_warning = 0;
    zucmotStatus->pre_check_joint_simple_tp = 0;
    zucmotStatus->check_safezone_prediction = 0;
    zucmotStatus->check_singular_7dof = 0;
    zucmotStatus->safezone_dengrous = 0;
    for (int i = 0; i < 6; ++i) { zucmotStatus->pre_joint_simple_tp[i] = 0.0; }
    zucmotStatus->attitude_limit_take_effect_flag = 0;
    zucmotStatus->emergency_stop = 0;
    zucmotStatus->safety_signal_abnormal = 0;
    zucmotStatus->category_one_stop_program_pause = 0;
    zucmotStatus->estop_recovery = 0;  //estop_recovery初始值不为零导致急停后恢复程序报动量超限问题

    ZERO_ZUC_POSE(zucmotStatus->carte_pos_cmd);
    ZERO_ZUC_POSE(zucmotStatus->carte_pos_fb);
    ZERO_ZUC_POSE(zucmotStatus->carte_pos_flange);
    ZERO_ZUC_POSE(zucmotConfig->tool_offset);
    ZERO_ZUC_POSE(zucmotConfig->user_offset);
    ZERO_ZUC_POSE(zucmotStatus->toolCoordOffset);
    zucmotStatus->vel = 0.0;
    zucmotConfig->limitVel = 0.0;
    zucmotStatus->acc = 0.0;
    zucmotStatus->feed_scale = 1.0;
    zucmotStatus->rapid_scale = 1.0;
    zucmotStatus->net_feed_scale = 1.0;
    /* adaptive feed is off by default, feed override, spindle 
       override, and feed hold are on */
    zucmotStatus->enables_new = FS_ENABLED | SS_ENABLED | FH_ENABLED;
    zucmotStatus->enables_queued = zucmotStatus->enables_new;
    zucmotStatus->id = 0;
    zucmotStatus->depth = 0;
    zucmotStatus->activeDepth = 0;
    zucmotStatus->paused = 0;
    zucmotStatus->overrideLimitMask = 0;

    zucmotStatus->admittance_enabled = 0;
    zucmotStatus->compliantEnable = 0;
    zucmotStatus->compliantType = 0;
    /*current_feedforward  includeGrvFlag */
    zucmotStatus->includeGrvFlag = -1;
    zucmotStatus->percentage_mode_level = 0;
    zucmotStatus->safetyguard_lock = 0;
    zucmotStatus->velcompscale = 1.0;
    zucmotStatus->momvelscale = 1.0;
    zucmotStatus->three_position_enable_limit = 0;
    zucmotStatus->tcp_force_limit = 0;
    zucmotStatus->last_momentum_movescale = 1.0;
    zucmotStatus->last_power_movescale = 1.0;
    zucmotStatus->is_momentumLimited = 0;
    zucmotStatus->is_robot_power_limited = 0;
    zucmotStatus->is_robot_endforce_limited = 0;
    zucmotStatus->power_protective_stop = 0;
    zucmotStatus->momentum_protective_stop = 0;
    zucmotStatus->maintenance_status = 0;

    for (int i = 0; i < ZUCMOT_MAX_JOINTS; i++)
    {
        zucmotStatus->joint_dir[i] = 1;
        zucmotStatus->joint_offset[i] = 0;
    }
    zucmotStatus->torque_control_settings.torque_control_enable = 0;
    zucmotStatus->torque_control_settings.torque_last_count = -1;
    zucmotStatus->torque_control_settings.command_to_close = 0;
    for (n = 0; n < ZUCMOT_MAX_JOINTS; n++) { zucmotStatus->torque_control_settings.current_torque_val[n] = 0; }

    /*initialize the composition factor*/
    zucmotStatus->admittance_enabled = 0;
    for (n = 0; n < 60; ++n) { zucmotStatus->dynParam.kineticK[n] = 0.0; }

    for (n = 0; n < 72; ++n) { zucmotStatus->dynParam.dynaticK[n] = 0.0; }

    zucmotStatus->dhParam = iniDHParam;

    zucmotStatus->funcIOMap.task_run_di.type = -1;
    zucmotStatus->funcIOMap.task_run_di.index = -1;
    zucmotStatus->funcIOMap.task_pause_di.type = -1;
    zucmotStatus->funcIOMap.task_pause_di.index = -1;
    zucmotStatus->funcIOMap.task_resume_di.type = -1;
    zucmotStatus->funcIOMap.task_resume_di.index = -1;
    zucmotStatus->funcIOMap.task_stop_di.type = -1;
    zucmotStatus->funcIOMap.task_stop_di.index = -1;
    zucmotStatus->funcIOMap.power_on_di.type = -1;
    zucmotStatus->funcIOMap.power_on_di.index = -1;
    zucmotStatus->funcIOMap.power_off_di.type = -1;
    zucmotStatus->funcIOMap.power_off_di.index = -1;
    zucmotStatus->funcIOMap.servo_enable_di.type = -1;
    zucmotStatus->funcIOMap.servo_enable_di.index = -1;
    zucmotStatus->funcIOMap.servo_disable_di.type = -1;
    zucmotStatus->funcIOMap.servo_disable_di.index = -1;
    zucmotStatus->funcIOMap.percentage_mode_level1_di.type = -1;
    zucmotStatus->funcIOMap.percentage_mode_level1_di.index = -1;
    zucmotStatus->funcIOMap.stop_mode_di.type = -1;
    zucmotStatus->funcIOMap.stop_mode_di.index = -1;
    zucmotStatus->funcIOMap.percentage_mode_level2_di.type = -1;
    zucmotStatus->funcIOMap.percentage_mode_level2_di.index = -1;
    zucmotStatus->funcIOMap.back_to_inipos_di.type = -1;
    zucmotStatus->funcIOMap.back_to_inipos_di.index = -1;

    zucmotStatus->funcIOMap.clear_error_di.type = -1;
    zucmotStatus->funcIOMap.clear_error_di.index = -1;  //
    zucmotStatus->funcIOMap.drag_mode_di.type = -1;
    zucmotStatus->funcIOMap.drag_mode_di.index = -1;
    zucmotStatus->funcIOMap.drag_mode_disable_di.type = -1;
    zucmotStatus->funcIOMap.drag_mode_disable_di.index = -1;
    for (int i = 0; i < 4; i++)
    {
        zucmotStatus->funcIOMap.task_idle_do[i].type = -1;
        zucmotStatus->funcIOMap.task_idle_do[i].index = -1;  //
        zucmotStatus->funcIOMap.task_paused_do[i].type = -1;
        zucmotStatus->funcIOMap.task_paused_do[i].index = -1;
        zucmotStatus->funcIOMap.task_running_do[i].type = -1;
        zucmotStatus->funcIOMap.task_running_do[i].index = -1;
        zucmotStatus->funcIOMap.stat_fault_do[i].type = -1;
        zucmotStatus->funcIOMap.stat_fault_do[i].index = -1;
        zucmotStatus->funcIOMap.stat_poweron_do[i].type = -1;
        zucmotStatus->funcIOMap.stat_poweron_do[i].index = -1;
        zucmotStatus->funcIOMap.stat_enabled_do[i].type = -1;
        zucmotStatus->funcIOMap.stat_enabled_do[i].index = -1;
        zucmotStatus->funcIOMap.stat_motion_do[i].type = -1;
        zucmotStatus->funcIOMap.stat_motion_do[i].index = -1;
        zucmotStatus->funcIOMap.stat_static_do[i].type = -1;
        zucmotStatus->funcIOMap.stat_static_do[i].index = -1;
        zucmotStatus->funcIOMap.stat_boot_do[i].type = -1;
        zucmotStatus->funcIOMap.stat_boot_do[i].index = -1;
        zucmotStatus->funcIOMap.stat_emergency_stop_do[i].type = -1;
        zucmotStatus->funcIOMap.stat_emergency_stop_do[i].index = -1;
        zucmotStatus->funcIOMap.stat_percentage_mode_level2_do[i].type = -1;
        zucmotStatus->funcIOMap.stat_percentage_mode_level2_do[i].index = -1;
        zucmotStatus->funcIOMap.stat_sys_protective_stop_do[i].type = -1;
        zucmotStatus->funcIOMap.stat_sys_protective_stop_do[i].index = -1;
        zucmotStatus->funcIOMap.stat_initial_pose_do[i].type = -1;
        zucmotStatus->funcIOMap.stat_initial_pose_do[i].index = -1;
        zucmotStatus->funcIOMap.stat_percentage_mode_level1_do[i].type = -1;
        zucmotStatus->funcIOMap.stat_percentage_mode_level1_do[i].index = -1;
        zucmotStatus->funcIOMap.stat_drag_mode_do[i].type = -1;
        zucmotStatus->funcIOMap.stat_drag_mode_do[i].index = -1;
        zucmotStatus->funcIOMap.stat_collision_mode_do[i].type = -1;
        zucmotStatus->funcIOMap.stat_collision_mode_do[i].index = -1;
        zucmotStatus->funcIOMap.stat_manual_mode_do[i].type = -1;
        zucmotStatus->funcIOMap.stat_manual_mode_do[i].index = -1;
        zucmotStatus->funcIOMap.stat_auto_mode_do[i].type = -1;
        zucmotStatus->funcIOMap.stat_auto_mode_do[i].index = -1;
    }

    SET_MOTION_INPOS_FLAG(1);
    SET_MOTION_ENABLE_FLAG(0);
    //added by alex at 2018.10.10 for can master
    // right now ,we have to output the enable signal to can master

    /* record the kinematics type of the machine */
    zucmotConfig->kinType = KINEMATICS_BOTH;
    zucmot_config_change();

    /* init pointer to joint structs */
#ifdef STRUCTS_IN_SHMEM
    motors = &(zucmotDebug->motors[0]);
    joints = &(zucmotDebug->joints[0]);
    axes = &(zucmotDebug->axes[0]);
#else
    motors = &(motor_array[0]);
    joints = &(joint_array[0]);
    axes = &(axis_array[0]);
#endif

    for (axis_num = 0; axis_num < ZUCMOT_MAX_AXIS; axis_num++)
    {
        zucmot_axis_t* axis;
        axis = &axes[axis_num];
        axis->locking_joint = -1;
    }
    /* init per-joint stuff */
    for (joint_num = 0; joint_num < zucmotConfig->numJoints; joint_num++)
    {
        /* point to structure for this joint */
        joint = &joints[joint_num];
        /* init the config fields with some "reasonable" defaults" */
        joint->type = 0;
        joint->max_pos_limit = 1.0;
        joint->min_pos_limit = -1.0;
        joint->vel_limit = 1.0;
        joint->acc_limit = 1.0;
        joint->min_ferror = 0.01;
        joint->max_ferror = 1.0;
        joint->home_search_vel = 0.0;
        joint->home_latch_vel = 0.0;
        joint->home_final_vel = -1;
        joint->home_offset = 0.0;
        joint->home = 0.0;
        joint->home_flags = 0;
        joint->home_sequence = -1;
        joint->home_state = HOME_IDLE;
        joint->backlash = 0.0;

        joint->comp.entries = 0;
        joint->comp.entry = &(joint->comp.array[0]);
        /* the compensation code has -DBL_MAX at one end of the table
        and +DBL_MAX at the other so _all_ commanded positions are
        guaranteed to be covered by the table */
        joint->comp.array[0].nominal = -DBL_MAX;
        joint->comp.array[0].fwd_trim = 0.0;
        joint->comp.array[0].rev_trim = 0.0;
        joint->comp.array[0].fwd_slope = 0.0;
        joint->comp.array[0].rev_slope = 0.0;
        for (n = 1; n < ZUCMOT_COMP_SIZE + 2; n++)
        {
            joint->comp.array[n].nominal = DBL_MAX;
            joint->comp.array[n].fwd_trim = 0.0;
            joint->comp.array[n].rev_trim = 0.0;
            joint->comp.array[n].fwd_slope = 0.0;
            joint->comp.array[n].rev_slope = 0.0;
        }

        /* init joint flags */
        joint->flag = 0;
        SET_JOINT_INPOS_FLAG(joint, 1);

        /* init status info */
        joint->coarse_pos = 0.0;
        joint->pos_cmd = 0.0;
        joint->vel_cmd = 0.0;
        joint->acc_cmd = 0.0;
        joint->backlash_corr = 0.0;
        joint->backlash_filt = 0.0;
        joint->backlash_vel = 0.0;
        joint->motor_pos_cmd = 0.0;
        joint->motor_pos_fb = 0.0;
        joint->abs_pos_fb = 0;
        joint->motor_curr_cmd = 0;
        joint->pos_fb = 0.0;
        joint->ferror = 0.0;
        joint->ferror_limit = joint->min_ferror;
        joint->ferror_high_mark = 0.0;

        /* init internal info */
        cubicInit(&(joint->cubic));
    }

    for (joint_num = 0; joint_num < zucmotConfig->numJoints; joint_num++)
    {
        motor = &motors[joint_num];
        motor->rated_curr = 0.0;
        motor->rated_torq = 0.1;
        motor->rated_trans_ratio = 1;
    }

    /*! \todo FIXME-- add zucmotError */
    zucmotDebug->cur_time = zucmotDebug->last_time = 0.0;
    zucmotDebug->start_time = etime();
    zucmotDebug->running_time = 0.0;

    /* init motion zucmotDebug coord_tp */
    //    tpManager_init();
    //    int errId = tpManager_tpinit(DEFAULT_TC_QUEUE_SIZE_TOPPRA, zucmotConfig->trajCycleTime, &zucmotTp_vtable);
    zucmotStatus->last_momentum_movescale = 1.0;
    zucmotStatus->last_power_movescale = 1.0;
    zucmotStatus->momvelscale = 1.0;
    ZERO_ZUC_POSE(zucmotStatus->dtg);
    SET_MOTION_INPOS_FLAG(1);
    // if (errId != 0)
    // {
    //     rtapi_print_msg(RTAPI_MSG_ERR, "MOTION: failed to create motion zucmotDebug coord_tp\n");
    //     return -1;
    // }

    double joint_pos_cmd[ZUCMOT_MAX_JOINTS] = {0}; /*by zxqi to record the corr joint position*/
                                                   //    tpManager_tpSetPos_JPos(&zucmotStatus->carte_pos_cmd, joint_pos_cmd);
                                                   //    tpManager_tpSetVMax(zucmotStatus->vel, zucmotStatus->vel);
                                                   //    tpManager_tpSetAmax(zucmotStatus->acc);

    zucmotStatus->tail = 0;

    // if (-1 == servojInitQueue(&zucmotDebug->servoj_que))
    // {
    //     rtapi_print_msg(RTAPI_MSG_ERR, "MOTION: failed to create motion zucmotDebug->servoj_que\n");
    //     return -1;
    // }

    rtapi_print_msg(RTAPI_MSG_INFO, "MOTION: init_comm_buffers() complete\n");
    return 0;
}

/* init_threads() creates realtime threads, exports functions to
   do the realtime control, and adds the functions to the threads.
*/

static void* traj_task(void* p_args)
{
    rtapi_print_msg(RTAPI_MSG_INFO, "MOTION: traj_task() starting...");
    pthread_setname_np(pthread_self(), "trajtask");
    while (1)
    {
        rtbus::schedule_wait();  //等待rtbus任务的发送的周期性信号

        pthread_setcancelstate(PTHREAD_CANCEL_DISABLE, NULL);

        // 同步rtrobot数据, 从rtrobot拿数据至motion
        for (size_t i = 0; i < rtdev::RtRobotMgr::instance().robot_num(); i++)
        {
            auto robot = rtdev::RtRobotMgr::instance().get_rtrobot(i);
            auto rob = axisgroup::AxisGroupMgr::instance().get_axisgroup(i);
            if (robot.get() && rob.get())
            {
                robot->get_rtrobot_status_for_motion(&rob->get_rtrobot_status());
            }
        }

        // 安全事件响应
        safety::SafetyEventWatch::instance().watch();

        motion_cmd_svr();
        zucmotCommandHandler(NULL);
        zucmotController(NULL);
        for (size_t i = 0; i < axisgroup::AxisGroupMgr::instance().group_num(); i++)
        {
            auto robot = axisgroup::AxisGroupMgr::instance().get_axisgroup(i);
            if (robot.get())
            {
                robot->update();
                robot->prepare_robot_status_for_task();  // 准备好给到task层的数据
            }
        }

        pthread_setcancelstate(PTHREAD_CANCEL_ENABLE, NULL);
        rtbus::schedule_send_ack();  //向rtbus任务发送ack信号

        //cancel check
        pthread_testcancel();
    }
    return NULL;
}

static int init_traj_thread()
{
    pthread_attr_t attr;
    pthread_attr_init(&attr);
    pthread_attr_setschedpolicy(&attr, SCHED_FIFO);
    struct sched_param param;
    memset(&param, 0, sizeof(param));
    param.sched_priority = MOTION_TASK_PRIO;
    pthread_attr_setschedparam(&attr, &param);
    pthread_attr_setinheritsched(&attr, PTHREAD_EXPLICIT_SCHED);
    cpu_set_t cpuset;
    CPU_ZERO(&cpuset);
    CPU_SET(MOTION_TASK_CPU, &cpuset);
    pthread_attr_setaffinity_np(&attr, sizeof(cpuset), &cpuset);
    pthread_attr_setscope(&attr, PTHREAD_SCOPE_SYSTEM);

    pthread_t tid;
#if (defined _DEBUG) && (1)
    int retval = pthread_create(&tid, NULL, traj_task, NULL);
#else
    int retval = pthread_create(&tid, &attr, traj_task, NULL);
#endif
    if (retval < 0)
    {
        rtapi_print_msg(RTAPI_MSG_ERR, "MOTION: failed to create %ld nsec servo thread\n", servo_period_nsec);
        return -1;
    }
    pthread_detach(tid);
    return 0;
}
static void* rtbus_task(void* parg)
{
    rtapi_print_msg(RTAPI_MSG_INFO, "MOTION: rtbus_task() starting...");
    pthread_setname_np(pthread_self(), "rtbustask");

    int period_count = 0;
    int ratio = traj_period_ratio;

    double servo_period_sec = servo_period_nsec / 1e9;
    struct timespec next;
    clock_gettime(CLOCK_MONOTONIC, &next);
    struct timespec dt;
    dt.tv_sec = 0;
    dt.tv_nsec = servo_period_nsec;

    int32_t cyc_max = INT32_MIN, cyc_min = INT32_MAX, cyc_cur = 0;
    timespec cyc_last = {0, 0};
    while (1)
    {
        timespec cyc_t0;
        clock_gettime(CLOCK_MONOTONIC, &cyc_t0);

        rtbus::BusMgr::get()->hook_function([&]() {
            safety::SafeIOMgr::instance().update();
            //FIXME: xxf 将这块插值处理放到单轴的rt_task_update中去处理
            if (period_count == 0)
            {
                period_count = ratio;
                for (size_t i = 0; i < axis::AxisMgr::get().count(); i++)
                {
                    auto axis = axis::AxisMgr::get().axis(i);
                    if (axis)
                    {
                        axis->rt_task_update();  //获取traj的规划指令数据并做插值处理
                    }
                }

                // update feedback data

                // axis data

                // robot data
                for (size_t i = 0; i < rtdev::RtRobotMgr::instance().robot_num(); i++)
                {
                    auto rtrobot = rtdev::RtRobotMgr::instance().get_rtrobot(i);
                    if (rtrobot.get())
                    {
                        rtrobot->prepare_status_for_motion();
                    }
                }
                rtbus::schedule_motion();
            }

            period_count--;
            auto ts = servo_period_sec * (ratio - period_count);

            rtdev::device_update(ts);

            for (size_t i = 0; i < axisgroup::AxisGroupMgr::instance().group_num(); i++)
            {
                auto robot = axisgroup::AxisGroupMgr::instance().get_axisgroup(i);
                robot->rt_over_plan_update(ts);  // 目前是专门处理servo模式的数据，update后axis-servo的数据会更改会经过滤波的用户servo指令

                auto rtrobot = rtdev::RtRobotMgr::instance().get_rtrobot(i);  // FIXME 最新的指令位置，和本周期的反馈位置？
                if (rtrobot.get())
                {
                    rtrobot->update();  // 发送伺服报文
                }
            }
        });
        timespec cyc_t1;
        clock_gettime(CLOCK_MONOTONIC, &cyc_t1);
        int64_t timeoff = 0;
        rtbus::BusMgr::get()->update(&timeoff);
        next = timespec_add(next, dt);
        next.tv_nsec += timeoff;
        timespec cyc_te;
        clock_gettime(CLOCK_MONOTONIC, &cyc_te);

        auto dev_handle_time = timespec_sub(cyc_t1, cyc_t0);
        auto bus_handle_time = timespec_sub(cyc_te, cyc_t1);
        bool do_print = false;
        auto cyc_time = timespec_sub(cyc_te, cyc_last);
        cyc_cur = cyc_time.tv_nsec + cyc_time.tv_sec * 1e9;
        if (cyc_last.tv_nsec || cyc_last.tv_nsec && cyc_cur > 0)
        {
            if (cyc_cur > cyc_max)
            {
                cyc_max = cyc_cur;
                do_print = true;
            }
            else if (cyc_cur < cyc_min)
            {
                cyc_min = cyc_cur;
                do_print = true;
            }
        }
        cyc_last = cyc_te;
        if (do_print)
        {
            zlog()->info("[RTBUS] cyc_max:{} cyc_min:{}", cyc_max, cyc_min);
            zlog()->info("[RTBUS] cyc_time:{} dev_handle_time:{} bus_handle_time:{}", cyc_cur, dev_handle_time.tv_nsec, bus_handle_time.tv_nsec);
        }

        clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &next, NULL);
    }
    return NULL;
}

int init_rtbus_thread()
{
    pthread_attr_t attr;
    pthread_attr_init(&attr);
    pthread_attr_setschedpolicy(&attr, SCHED_FIFO);
    struct sched_param param;
    memset(&param, 0, sizeof(param));
    param.sched_priority = RTBUS_TASK_PRIO;
    pthread_attr_setschedparam(&attr, &param);
    pthread_attr_setinheritsched(&attr, PTHREAD_EXPLICIT_SCHED);
    cpu_set_t cpuset;
    CPU_ZERO(&cpuset);
    CPU_SET(RTBUS_TASK_CPU, &cpuset);
    pthread_attr_setaffinity_np(&attr, sizeof(cpuset), &cpuset);
    pthread_attr_setscope(&attr, PTHREAD_SCOPE_SYSTEM);

    pthread_t tid;
#if (defined _DEBUG) && (1)
    int retval = pthread_create(&tid, NULL, rtbus_task, NULL);
#else
    int retval = pthread_create(&tid, &attr, rtbus_task, NULL);
#endif
    if (retval < 0)
    {
        rtapi_print_msg(RTAPI_MSG_ERR, "RTBUS: failed to create %ld nsec servo thread\n", servo_period_nsec);
        return -1;
    }
    pthread_detach(tid);
    return 0;
}
static int init_threads(void)
{
    rtapi_print_msg(RTAPI_MSG_INFO, "MOTION: init_threads() starting...\n");

    traj_period_nsec = traj_period_ratio * servo_period_nsec;
    for (size_t i = 0; i < axisgroup::AxisGroupMgr::instance().group_num(); i++)
    {
        auto robot = axisgroup::AxisGroupMgr::instance().get_axisgroup(i);
        robot->set_cycle_time(traj_period_nsec * 1e-9, servo_period_nsec * 1e-9);
    }

    safety::SafetyEventWatch::instance().init(1.0 / (traj_period_nsec * 1e-9));

    init_traj_thread();
    // if we don't set cycle times, zuc doesn't start up right
    setServoCycleTime(servo_period_nsec * 1e-9);
    setTrajCycleTime(traj_period_nsec * 1e-9);
    rtapi_print_msg(RTAPI_MSG_INFO, "MOTION: init_threads() complete\n");
    return 0;
}

/* call this when setting the trajectory cycle time */
static int setTrajCycleTime(double secs)
{
    static int t;

    rtapi_print_msg(RTAPI_MSG_INFO, "MOTION: setting Traj cycle time to %ld nsecs\n", (long)(secs * 1e9));

    /* make sure it's not zero */
    if (secs <= 0.0)
    {
        return -1;
    }
    zucmot_config_change();

    zucmotConfig->interpolationRate = traj_period_ratio;

    /* set traj planner */
    //    tpManager_tpSetCycleTime(secs);

    /* set the free planners, cubic interpolation rate and segment time */
    for (int t = 0; t < zucmotConfig->numJoints; t++) { cubicSetInterpolationRate(&(joints[t].cubic), 1); }

    /* copy into status out */
    zucmotConfig->trajCycleTime = secs;

    return 0;
}

/* call this when setting the servo cycle time */
static int setServoCycleTime(double secs)
{
    static int t;

    rtapi_print_msg(RTAPI_MSG_INFO, "MOTION: setting Servo cycle time to %ld nsecs\n", (long)(secs * 1e9));

    /* make sure it's not zero */
    if (secs <= 0.0)
    {
        return -1;
    }

    zucmot_config_change();

    /* compute the interpolation rate as nearest integer to traj/servo */
    zucmotConfig->interpolationRate = traj_period_ratio;

    /* set the cubic interpolation rate and PID cycle time */
    for (t = 0; t < zucmotConfig->numJoints; t++)
    {
        cubicSetInterpolationRate(&(joints[t].cubic), 1);
        cubicSetSegmentTime(&(joints[t].cubic), secs);
    }

    /* copy into status out */
    zucmotConfig->servoCycleTime = secs;

    return 0;
}

/* @brief set or reset the bit to indicate Drag mode
 *
 * This bit is set for canusr component, when this flag 
 * is set, the system goes into the teach-drag mode,
 * and the position data from controller will not be
 * sent to drives. Instead, the data from the Zero_G 
 * component will be sent to drive.
 * 
 * @param bStat the target status of the hal bit
 * */

void setDragModeBit(bool bStat)
{
    if (bStat)
    {
        zucmot_hal_data->drag_mode = 1;
    }
    else
    {
        zucmot_hal_data->drag_mode = 0;
    }
}
